#ifndef __SENSOR_BASE_H_
#define __SENSOR_BASE_H_

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h> // sensor_msgs::convertPointCloud2ToPointCloud
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// #include <pcl/conversions.h>

// 记一次提高依赖PCL的C++代码编译速度的经历 https://blog.csdn.net/tfb760/article/details/134122703

#include <mc_core/common/glog.h>

using namespace std;
using namespace Eigen;

// 定义基础数据类型
using Scan2D = sensor_msgs::LaserScan;
using Scan2DPtr = Scan2D::Ptr;
using PointCloudXYZ = pcl::PointCloud<pcl::PointXYZ>;
using PointCloudXYZPtr = pcl::PointCloud<pcl::PointXYZ>::Ptr;
using PointCloud2 = sensor_msgs::PointCloud2;
using PointCloud2Ptr = sensor_msgs::PointCloud2::Ptr;
using Imu = sensor_msgs::Imu;
using ImuPtr = sensor_msgs::Imu::Ptr;
using Odometry = nav_msgs::Odometry;
using OdometryPtr = nav_msgs::Odometry::Ptr;

// typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
// typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudXYZPtr;
// typedef sensor_msgs::PointCloud2 PointCloud2;
// typedef sensor_msgs::PointCloud2::Ptr PointCloud2Ptr;

// 传感器数据配置
struct SeneorConfig{
    string topic_name = "";             // 话题名称
    string frame_id = "";               // 坐标系
    Eigen::Affine3f transform_matrix;   // 坐标系转换 base_link -> frame_id
    bool enable = true;                 // 是否启用
    bool has_transform = false;         // 是否需要坐标系转换
    int index = 0;                      // 传感器编号
};


namespace msg_convert{

    // geometry_msgs::TransformStamped -> Eigen::Affine3f
    Eigen::Affine3f Geometry2Affine(geometry_msgs::TransformStamped transform);

    // 转换2D数据为点云数据 Scan2DPtr -> PointCloudXYZPtr
    PointCloudXYZPtr convertToPointCloudXYZ(
        Scan2DPtr scan_ptr);


    // 转换点云数据  PointCloud2Ptr -> PointCloudXYZPtr
    PointCloudXYZPtr convertToPointCloudXYZ(
        PointCloud2Ptr pointcloud2_ptr);

    // 转换到PointCloud2  PointCloudXYZPtr -> sensor_msgs::PointCloud2
    sensor_msgs::PointCloud2Ptr convertToPointCloud2(PointCloudXYZPtr pointcloud_ptr);

    // TF变换 pcl::PointCloud<pcl::PointXYZ>::Ptr  Eigen::Affine3f
    void transformPointXYZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, const Eigen::Affine3f &transform);  

    // sensor_msgs::PointCloud2::Ptr -> sensor_msgs::PointCloud
    sensor_msgs::PointCloud convertToPointCloud(sensor_msgs::PointCloud2Ptr pointcloud2_ptr);

};  // namespace

#endif